#include "findingPath.h"
#include "PositionInformation.h"
#include "StateMachine.h"


vector<vector<char> > search_map;

bool activ_path = false;

//saint_nr_9::RobotPosition robo_position;

vector<vector<map_element> > state_map;

vector<vec2D> directions;

ros::Publisher pub_state_change;

int global_state = 0;

void readMap(string path)
{
  string line;
  std::ifstream myfile (path.c_str());
  
    if (myfile.is_open())
    { 
      //printf("Hui map file opened :) \n");
      //string ha = "haha";
      //cout << ha.compare("haha")<<endl;
      
      while ( myfile.good() )
      {
        printf("Hui map file opened :) \n");
        std::getline (myfile,line);
        //cout << line << " " << "Wallcompare: " << line.compare("WALLS") << "\n" <<endl;
        //line = "WALLS";
        //cout << line.compare("WALLS")<<endl;
        
        if (line.compare(0,5,"WALLS")==0)
        {
          //printf("Juhu \n");
          break;
        }
        //cout << line <<endl;
        search_map.push_back(vector<char>());
        state_map.push_back(vector<map_element>());
        for (int i = 0; i < (int)line.size();i++)
        {
          search_map.back().push_back(line[i]);
          state_map.back().push_back(map_element());
        }
      }
      myfile.close();
    }
    
    
    /*for (int i = 0; i < search_map.size();i++)
    {
      for (int j = 0; j < search_map.at(i).size(); j++)
      {
        cout << search_map.at(i).at(j);
      }
      cout << endl;
    } */  
}

int getDirection(vec2D force)
{
  if (force.x != 0) return X_DIRECTION;
  else return Y_DIRECTION;
}

void reset_field_forces()
{
  for (int x = 0; x < state_map.size(); x++ )
  {
    for (  int y = 0; y < state_map[x].size(); y++)
    {
      state_map[x][y].force.x = -2;
      state_map[x][y].force.y = -2;      
    }
  }   
}

bool same_direction(vec2D force, int start_direc)
{
  return ((start_direc == X_DIRECTION && force.x!=0) || (start_direc == Y_DIRECTION && force.y!=0));
}

void computeFollowers(int x, int y, int start_direc, int value, priority_queue<force_node, vector<force_node>, Compare_force_node> &pq)
{
  for (int i = 0; i < directions.size(); i++ )
  {
    force_node n1;
    n1.position.x = x+directions[i].x;
    n1.position.y = y+directions[i].y;    
    n1.force.x = directions[i].x*-1;    
    n1.force.y = directions[i].y*-1;
    if(same_direction(n1.force, start_direc))
    {
      n1.value = value; 
    }
    else
    {
      n1.value = value-1;
    }
    pq.push(n1);
  }
}

void computeForces(int x_start, int y_start, int x_length, int y_length, int start_direc)
{
  priority_queue<force_node, vector<force_node>, Compare_force_node> pq;
  //cout<<"Hello"<<endl;       
  for (int x = x_start; x < (x_start+x_length); x++ )
  {
    for (int y = y_start; y < (y_start + y_length); y++ )
    {
      computeFollowers(x, y, start_direc, 20, pq);
      //cout<<"Hello3"<<endl;       
      state_map[x][y].force.x = 0;
      state_map[x][y].force.y = 0;      
    }
  }
  //cout<<"Hello4"<<endl;       
  while(!pq.empty())
  {
    force_node n1 = pq.top();
    vec2D pos = n1.position;
    if (state_map[pos.x][pos.y].force.x == -2 && search_map[pos.x][pos.y] == FREE_SPACE)   
    {
      state_map[pos.x][pos.y].force = n1.force;
      pq.pop();
      computeFollowers(pos.x, pos.y, getDirection(n1.force), n1.value, pq);
    }
    else
    {
      pq.pop();    
    }

  }
}

void set_goal_area(const saint_nr_9::GoalArea::ConstPtr& area)
{
  reset_field_forces();
  computeForces(area->x, area->y, area->x_length, area->y_length, area->start_direction);
  activ_path = true;
  cout << "Set Area"<<endl;
  ofstream myfile ("potentialfield.txt");
  if (myfile.is_open())
  {
    for (int x = 0; x < state_map.size(); x++ )
    {
      for (int y = 0; y < state_map[x].size(); y++)
      {
        myfile << state_map[x][y].print();
      }  
      myfile <<"\n";
  }
    myfile.close();
  }
}

int needed_turn(vec2D force, int heading)
{
  if(force.x == up.x && force.y == up.y)
  {
    cout << "UP but need turn" << endl;
    if(heading == POS_LEFT) return PATH_RIGHT_TURN;
    if(heading == POS_DOWN) return PATH_U_TURN;
    if(heading == POS_RIGHT) return PATH_LEFT_TURN;
  }
  if(force.x == right.x && force.y == right.y)
  {
    cout << "RIGHT but need turn" << endl;
    if(heading == POS_LEFT) return PATH_U_TURN;
    if(heading == POS_DOWN) return PATH_RIGHT_TURN;
    if(heading == POS_UP) return PATH_LEFT_TURN;
  }
  if(force.x == left.x && force.y == left.y)
  {
    cout << "LEFT but need turn" << endl;
    if(heading == POS_RIGHT) return PATH_U_TURN;
    if(heading == POS_DOWN) return PATH_RIGHT_TURN;
    if(heading == POS_UP) return PATH_LEFT_TURN;
  }  
  if(force.x == down.x && force.y == down.y)
  {
    cout << "DOWN but need turn" << endl;  
    if(heading == POS_RIGHT) return PATH_RIGHT_TURN;
    if(heading == POS_LEFT) return PATH_LEFT_TURN;
    if(heading == POS_UP) return PATH_U_TURN;    
  }   
  
  if(force.x == 0 && force.y == 0)
  {
    printf("Shit undefined needed turn\n");
  }
  return PATH_SEARCH_TAG;


}


bool equal_heading(vec2D force, int heading)
{
  if(force.x == up.x && force.y == up.y && heading == POS_UP) return true;
  if(force.x == right.x && force.y == right.y && heading == POS_RIGHT) return true;
  if(force.x == left.x && force.y == left.y && heading == POS_LEFT) return true;
  if(force.x == down.x && force.y == down.y && heading == POS_DOWN) return true;
  return false;
}

void update_position(const saint_nr_9::RobotPosition::ConstPtr& position)
{
 //cout << "position"   << endl;
  if (activ_path)
  {
    if(global_state != PATH_STRAIGHT)
    {
        std_msgs::Int32 p;
        p.data = PATH_STRAIGHT;
        pub_state_change.publish(p);
    }
    else
    {  
      printf("Hui a position aand an active path :) \n");
      if(equal_heading(state_map[position->x][position->y].force, position->heading))
      {
        if(global_state != PATH_STRAIGHT)
        {
          std_msgs::Int32 p;
          p.data = PATH_STRAIGHT;
          pub_state_change.publish(p);
        }
      }
      else
      {
        std_msgs::Int32 p;
        p.data = needed_turn(state_map[position->x][position->y].force, position->heading);
        pub_state_change.publish(p);
      }
    }
  }
  
  ros::spinOnce();
    
    
}


void state_callback(const std_msgs::Int32::ConstPtr& state)
{
  global_state = state->data;
}


void initialize_map(const std_msgs::String::ConstPtr& signal)
{
  printf("Hui a map :) \n");
  readMap(signal->data);
}

int main(int argc, char **argv)
{
	printf("Starting PathFinder\n");
	ros::init(argc, argv, "PathFinder");
	ros::NodeHandle n;
	directions.push_back(vec2D(0,1));
	directions.push_back(vec2D(1,0));
	directions.push_back(vec2D(0,-1));	
	directions.push_back(vec2D(-1,0));
	
	ros::Subscriber sub_path = n.subscribe("Signals/map_path", 1, initialize_map);
	ros::Subscriber sub_position = n.subscribe("/position", 1, update_position);	
	ros::Subscriber sub_goal = n.subscribe("/goal_area", 1, set_goal_area);		
  ros::Subscriber state_sub = n.subscribe("State/Info", 1, state_callback);	
  pub_state_change = n.advertise<std_msgs::Int32>("Signals/state_path",2);
  /*ofstream myfile ("potentialfield.txt");
  if (myfile.is_open())
  {
    for (int x = 0; x < state_map.size(); x++ )
    {
      for (int y = 0; y < state_map[x].size(); y++)
      {
        myfile << state_map[x][y].print();
      }  
      myfile <<"\n";
  }
    myfile.close();
  }*/
  
  //printf("Done\n");
  ros::spin();
}
